package gpstools.math;

import gpstools.Location;
import gpstools.Point;

public class GeoCalculations {
    /**
     * http://stackoverflow.com/questions/837872/calculate-distance-in-meters-when-you-know-longitude-and-latitude-in-java
     */
    public static double distance(double lat1, double lon1, double lat2, double lon2) {
        double earthRadius = 3958.75;
		double dLat = Math.toRadians(lat2 - lat1);
		double dLng = Math.toRadians(lon2 - lon1);
		double a = Math.sin(dLat / 2) * Math.sin(dLat / 2) + Math.cos(Math.toRadians(lat1))
				* Math.cos(Math.toRadians(lat2)) * Math.sin(dLng / 2) * Math.sin(dLng / 2);
		double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
		double dist = earthRadius * c;
		
		int meterConversion = 1609;
		
		return new Float(dist * meterConversion).floatValue();
    }

	public static float distFrom(double lat1, double lng1, double lat2, double lng2) {
		double earthRadius = 3958.75;
		double dLat = Math.toRadians(lat2 - lat1);
		double dLng = Math.toRadians(lng2 - lng1);
		double a = Math.sin(dLat / 2) * Math.sin(dLat / 2) + Math.cos(Math.toRadians(lat1))
				* Math.cos(Math.toRadians(lat2)) * Math.sin(dLng / 2) * Math.sin(dLng / 2);
		double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
		double dist = earthRadius * c;

		int meterConversion = 1609;

		return new Float(dist * meterConversion).floatValue();
	}

    /**
     * Returns the distance of p3 to the segment defined by p1,p2;
     * 
     * @param p1
     *            First point of the segment
     * @param p2
     *            Second point of the segment
     * @param p3
     *            Point to which we want to know the distance of the segment
     *            defined by p1,p2
     * @return The distance of p3 to the segment defined by p1,p2
     */
    public static double distanceToSegment(double lat, double lon, double segLat1, double segLon1, double segLat2,
            double segLon2) {
        if ((segLat2 - segLat1 == 0) && (segLon2 - segLon1 == 0)) {
            return distance(lat, lon, segLat1, segLon1);
        }

        Location closest = closestSegmentPoint(lat, lon, segLat1, segLon1, segLat2, segLon2);

        return distance(closest.getLatitude(), closest.getLongitude(), lat, lon);
    }

    public static Location closestSegmentPoint(double lat, double lon, double segLat1, double segLon1, double segLat2,
            double segLon2) {
        final double latDelta = segLat2 - segLat1;
        final double lonDelta = segLon2 - segLon1;

        final double u = ((lat - segLat1) * latDelta + (lon - segLon1) * lonDelta)
                / (latDelta * latDelta + lonDelta * lonDelta);

        double closestLat;
        double closestLon;
        if (u < 0) {
            closestLat = segLat1;
            closestLon = segLon1;
        } else if (u > 1) {
            closestLat = segLat2;
            closestLon = segLon2;
        } else {
            closestLat = segLat1 + u * latDelta;
            closestLon = segLon1 + u * lonDelta;
        }
        return new Point(closestLat, closestLon);
    }

}
